#include "HkpShapeFactory.h"

namespace Havok
{
	namespace Collision
	{
		hkpShape* createBoxShape(float extents[])
		{
			hkVector4 size(extents[0], extents[1], extents[2]);
			return new hkpBoxShape(size, 0);
		}
		
		HkpShape^ HkpShapeFactory::CreateBox(Vector3 extents)
		{
			float* size = new float[3];
			size[0] = extents.X;
			size[1] = extents.Y;
			size[2] = extents.Z;

			hkpShape* shape = createBoxShape(size);

			delete size;

			return gcnew HkpShape(shape);
		}
		
		hkpShape* createSphereShape(float radius)
		{
			return new hkpSphereShape(radius);
		}
		
		HkpShape^ HkpShapeFactory::CreateSphere(float radius)
		{
			hkpShape* shape = createSphereShape(radius);
			return gcnew HkpShape(shape);
		}
		
		hkpShape* createTriangleShape(float vertexA[], float vertexB[], float vertexC[])
		{
			hkVector4 vertA(vertexA[0], vertexA[1], vertexA[2]);
			hkVector4 vertB(vertexB[0], vertexB[1], vertexB[2]);
			hkVector4 vertC(vertexC[0], vertexC[1], vertexC[2]);

			return new hkpTriangleShape(vertA, vertB, vertC, 0);
		}
		
		HkpShape^ HkpShapeFactory::CreateTriangle(Vector3 vertexA, Vector3 vertexB, Vector3 vertexC)
		{
			float* vertA = new float[3];
			float* vertB = new float[3];
			float* vertC = new float[3];

			vertA[0] = vertexA.X;
			vertA[1] = vertexA.Y;
			vertA[2] = vertexA.Z;
			vertB[0] = vertexB.X;
			vertB[1] = vertexB.Y;
			vertB[2] = vertexB.Z;
			vertC[0] = vertexC.X;
			vertC[1] = vertexC.Y;
			vertC[2] = vertexC.Z;

			hkpShape* shape = createTriangleShape(vertA, vertB, vertC);

			delete vertA;
			delete vertB;
			delete vertC;

			return gcnew HkpShape(shape);
		}
		
		hkpShape* createCapsuleShape(float vertexA[], float vertexB[], float radius)
		{
			hkVector4 vertA(vertexA[0], vertexA[1], vertexA[2]);
			hkVector4 vertB(vertexB[0], vertexB[1], vertexB[2]);

			return new hkpCapsuleShape(vertA, vertB, radius);
		}
		
		HkpShape^ HkpShapeFactory::CreateCapsule(Vector3 vertexA, Vector3 vertexB, float radius)
		{
			float* vertA = new float[3];
			float* vertB = new float[3];

			vertA[0] = vertexA.X;
			vertA[1] = vertexA.Y;
			vertA[2] = vertexA.Z;
			vertB[0] = vertexB.X;
			vertB[1] = vertexB.Y;
			vertB[2] = vertexB.Z;

			hkpShape* shape = createCapsuleShape(vertA, vertB, radius);

			delete vertA;
			delete vertB;

			return gcnew HkpShape(shape);
		}
		
		hkpShape* createCylinderShape(float vertexA[], float vertexB[], float radius)
		{
			hkVector4 vertA(vertexA[0], vertexA[1], vertexA[2]);
			hkVector4 vertB(vertexB[0], vertexB[1], vertexB[2]);

			return new hkpCylinderShape(vertA, vertB, radius);
		}
		
		HkpShape^ HkpShapeFactory::CreateCylinder(Vector3 vertexA, Vector3 vertexB, float radius)
		{
			float* vertA = new float[3];
			float* vertB = new float[3];

			vertA[0] = vertexA.X;
			vertA[1] = vertexA.Y;
			vertA[2] = vertexA.Z;
			vertB[0] = vertexB.X;
			vertB[1] = vertexB.Y;
			vertB[2] = vertexB.Z;

			hkpShape* shape = createCylinderShape(vertA, vertB, radius);

			delete vertA;
			delete vertB;

			return gcnew HkpShape(shape);
		}
		
		void createMeshPart(float vertexBuffer[], int numVertices, int indexBuffer[], int numIndices, hkpExtendedMeshShape* shape)
		{
			hkpExtendedMeshShape::TrianglesSubpart part;
			part.m_vertexBase = vertexBuffer;
			part.m_vertexStriding = (3 * sizeof(float));
			part.m_numVertices = numVertices;

			part.m_indexBase = indexBuffer;
			part.m_stridingType = hkpExtendedMeshShape::INDICES_INT32;
			part.m_indexStriding = 3 * sizeof(int);
			part.m_numTriangleShapes = numIndices / 3;

			shape->addTrianglesSubpart(part);
		}
		
		void createMeshPart(float vertexBuffer[], int numVertices, short indexBuffer[], int numIndices, hkpExtendedMeshShape* shape)
		{
			hkpExtendedMeshShape::TrianglesSubpart part;
			part.m_vertexBase = vertexBuffer;
			part.m_vertexStriding = (3 * sizeof(float));
			part.m_numVertices = numVertices;

			part.m_indexBase = indexBuffer;
			part.m_stridingType = hkpExtendedMeshShape::INDICES_INT16;
			part.m_indexStriding = (3 * sizeof(short));
			part.m_numTriangleShapes = numIndices / 3;

			shape->addTrianglesSubpart(part);
		}
		
		HkpShape^ HkpShapeFactory::CreateTriangleMesh(Model^ model, bool useBones)
		{			
			hkpExtendedMeshShape* meshShape = new hkpExtendedMeshShape(0.01f);
			
			// Bone transformations
			array<Matrix>^ boneTransforms = gcnew array<Matrix>(model->Bones->Count);
			model->CopyAbsoluteBoneTransformsTo(boneTransforms);

			for (int i = 0; i < model->Meshes->Count; ++i)
			{
				ModelMesh^ modelMesh = model->Meshes[i];

				for (int x = 0; x < modelMesh->MeshParts->Count; ++x)
				{
					HkpMeshBuffer^ pMeshBuffer = MeshUtilities::GetMeshPartMeshBuffer(modelMesh, x);

					List<Vector3>^ vertices = pMeshBuffer->VertexBuffer;
					

					// If we want to use bones, transform every vertex by its absolute parent transform
					Matrix transform= boneTransforms[modelMesh->ParentBone->Index];
					
					for(int i = 0; i < vertices->Count; i++)
					{
						vertices[i] = Vector3::Transform(vertices[i],transform);
					}

					float* VertexBuffer = new float[vertices->Count * 3];

					int currentElement = 0;

					for (int y = 0; y < vertices->Count; ++y)
					{
						VertexBuffer[currentElement] = vertices[y].X;
						VertexBuffer[currentElement + 1] = vertices[y].Y;
						// Have to be -Z.. for some reason its swaped .. maybe wrong ??
 						VertexBuffer[currentElement + 2] = -vertices[y].Z;

						currentElement += 3;
					}

					if (pMeshBuffer->IndexSize == IndexElementSize::SixteenBits)
					{
						List<short>^ indices = pMeshBuffer->IndexBuffer;
						short* IndexBuffer = new short[indices->Count];

						for (int y = 0; y < indices->Count; ++y)
						{
							IndexBuffer[y] = indices[y];
						}

						createMeshPart(VertexBuffer, vertices->Count, IndexBuffer, indices->Count, meshShape);
					}
					else if (pMeshBuffer->IndexSize == IndexElementSize::ThirtyTwoBits)
					{
						List<int>^ indices = pMeshBuffer->IndexBuffer32Bit;
						int* IndexBuffer = new int[indices->Count];

						for (int y = 0; y < indices->Count; ++y)
						{
							IndexBuffer[y] = indices[y];
						}
						
						createMeshPart(VertexBuffer, vertices->Count, IndexBuffer, indices->Count, meshShape);
					}
				}
			}

			return gcnew HkpShape(meshShape);
		}
		
		HkpShape^ HkpShapeFactory::CreateMoppBvTreeForMesh(HkpShape^ meshShape)
		{
			hkpMoppCompilerInput* mci = new hkpMoppCompilerInput();

			hkpMoppCode* m_code = hkpMoppUtility::buildCode(((hkpExtendedMeshShape*)meshShape->m_shape), *mci);

			hkpMoppBvTreeShape* shape = new hkpMoppBvTreeShape(((hkpExtendedMeshShape*)meshShape->m_shape), m_code);

			delete mci;
			//delete m_code;

			return gcnew HkpShape(shape);
		}
	}
}